import rospy
import math
import time
from sensor_msgs.msg import LaserScan
#from geometry_msgs.msg import Twist
from std_msgs.msg import String
import os
import sys
path = os.path.abspath(".")
sys.path.insert(0,path + "/src/car_pkg/scripts")
import USART
sys.path.insert(0,"/home/sunrise/workspace/yolov5-master/")
from detect import biaozhi


sender = USART.SerialSender("/dev/ttyS3", 9600)
print("/dev/ttyS3;9600")
received_array=[1,1,1,1,1,1]
read = 0.0
scan_distance = 0.0

def index(a,a_min,a_inc):
    return int(((a*math.pi)/180 - a_min)/a_inc)

def lidar_back(data):
    global scan_distance
    scan_filter = [ ]
    for i in range(index(170,data.angle_min,data.angle_increment),index(180,data.angle_min,data.angle_increment)):
        if data.ranges[i] > 0.1:
            scan_filter.append(data.ranges[i])
    for i in range(index(-180,data.angle_min,data.angle_increment),index(-170,data.angle_min,data.angle_increment)):
        if data.ranges[i] > 0.1:
            scan_filter.append(data.ranges[i])
    #rospy.loginfo("Distance :"+ str(min(scan_filter)))
    scan_distance = min(scan_filter)


def lidar_yolo():
    global read
    global scan_distance
    scan_filter = [ ]
    if biaozhi ==0:
        try:
            with open(r"/home/sunrise/workspace/share_zuobiao.txt", mode="r") as file:
                lines = file.readlines()
            if lines and lines[0][0] == '1':
                if len(lines) > 1:
                    read1 = lines[1]
                    read = float(read1)
                lines[0] = '0' + lines[0][1:]
                with open(r"/home/sunrise/workspace/share_zuobiao.txt", 'w') as file:
                    file.writelines(lines)
                print(read)
                rospy.loginfo("Distance :"+ str(scan_distance))      
                if scan_distance < 0.6:
                    received_array[0]=0xFF
                    received_array[1]=0x01
                    received_array[2]=0x00
                    received_array[3]=0x00
                    received_array[4]=0x05
                    received_array[5]=0xAA
                    sender.air_speed(received_array)
                elif read > 380:
                    received_array[0]=0xFF
                    received_array[1]=0x01
                    received_array[2]=0x00
                    received_array[3]=0x01
                    received_array[4]=0x04
                    received_array[5]=0xAA
                    sender.air_speed(received_array)
                elif read < 310:
                    received_array[0]=0xFF
                    received_array[1]=0x01
                    received_array[2]=0x00
                    received_array[3]=0x01
                    received_array[4]=0x03
                    received_array[5]=0xAA
                    sender.air_speed(received_array)
                else :
                    #time.sleep(1)
                    print(scan_distance)
                    if scan_distance > 0.6 :
                        received_array[0]=0xFF
                        received_array[1]=0x01
                        received_array[2]=0x00
                        received_array[3]=0x02
                        received_array[4]=0x01
                        received_array[5]=0xAA
                        sender.air_speed(received_array)
                    else :
                        received_array[0]=0xFF
                        received_array[1]=0x01
                        received_array[2]=0x00
                        received_array[3]=0x00
                        received_array[4]=0x05
                        received_array[5]=0xAA
                        sender.air_speed(received_array)
   #####               
        except FileNotFoundError:
            print("not file")
    """
    for i in range(index(150,data.angle_min,data.angle_increment),index(180,data.angle_min,data.angle_increment)):
      if data.ranges[i] > 0.1:
        scan_filter.append(data.ranges[i])
    for i in range(index(-180,data.angle_min,data.angle_increment),index(-150,data.angle_min,data.angle_increment)):
      if data.ranges[i] > 0.1:
        scan_filter.append(data.ranges[i])
    rospy.loginfo("Distance :"+ str(min(scan_filter)))
    if min(scan_filter) < 0.4 : 
        msg.data = msg1
        received_array[0]=0xFF
        received_array[1]=0x01
        received_array[2]=0x00
        received_array[3]=0x00
        received_array[4]=0x05
        received_array[5]=0xAA
        sender.air_speed(received_array)
    elif min(scan_filter) > 0.4:
        msg.data = msg2
        received_array[0]=0xFF
        received_array[1]=0x01
        received_array[2]=0x00
        received_array[3]=0x02
        received_array[4]=0x01
        received_array[5]=0xAA
        sender.air_speed(received_array)
    pub.publish(msg) 
    """ 
def main_loop(event):
    global scan_distance
    if scan_distance is not None:
        lidar_yolo_wrapper()

def lidar_yolo_wrapper():
    lidar_yolo()
def main():
    rospy.init_node('laserscan_node',anonymous =False)
    rospy.Subscriber('/scan',LaserScan,lidar_back,queue_size =10)
    
    rospy.Timer(rospy.Duration(0.3),main_loop)
    
    rospy.spin()

if __name__ == "__main__":
    main()
        
    
